# -*- coding: utf-8 -*-
# @Time    : 2023/8/6 13:34
# @Author  : Z.J. Zhang
# @Email   : zijingzhang@mail.ustc.edu.cn
# @File    : vertex_position.py
# @Software: PyCharm
import typing
from collections import OrderedDict

import FreeCAD as App
import FreeCADGui as Gui
import Part
import numpy


# def get_selected_face() -> typing.List[Part.Face]:
#     """
#     获取选中的多个面
#     :return:
#     """
#     return Gui.Selection.getSelectionEx()[0].SubObjects


def faces_to_list_of_OuterWire_OrderedVertexes(faces: typing.List[Part.Face]) -> typing.List[
    typing.List[Part.Vertex]
]:
    res = []
    for face in faces:
        res.append(face.OuterWire.OrderedVertexes)
    return res


def outwire_vertexes_to_ndarray(
        list_of_OutWire_OrderedVertexes: typing.List[Part.Vertex]
) -> numpy.ndarray:
    res = []
    for OutWire_OrderedVertexes in list_of_OutWire_OrderedVertexes:
        coords = []
        for vertex in OutWire_OrderedVertexes:
            coords.append([vertex.X, vertex.Y, vertex.Z])
        res.append(coords)
    return numpy.array(res)


def get_coord_system_of_sketch(sketch_obj, ) -> typing.Tuple[Part.Vertex]:
    """

    :param sketch_obj:
    :return: 草图上的(1,0), (0,0), (0,1)的三维坐标
    """
    sketch_obj.addGeometry(
        Part.LineSegment(App.Vector(1, 0, 0), App.Vector(0, 0, 0)), False)  # horizontal axis (unit length)
    sketch_obj.addGeometry(
        Part.LineSegment(App.Vector(0, 0, 0), App.Vector(0, 1, 0)), False)  # Vertical axis (unit length)
    App.ActiveDocument.recompute()
    # tv = sketch_obj.ViewObject.TempoVis
    # if tv:
    #     tv.restore()
    # sketch_obj.ViewObject.TempoVis = None
    # del (tv)
    Gui.Selection.clearSelection()
    # Selection in 3D view
    for i in range(3):
        Gui.Selection.addSelection(App.ActiveDocument.Name, App.ActiveDocument.Body.Name,
                                   '%s.Vertex%d' % (sketch_obj.Name, i + 1))
    vertexes = Gui.Selection.getSelectionEx()[0].SubObjects

    # 删除原来的草图
    Gui.Selection.clearSelection()
    # del (sketch_obj)
    # Gui.Selection.clearSelection()
    # ActiveSketch = App.getDocument('Unnamed').getObject('Sketch')
    # tv = Show.TempoVis(App.ActiveDocument, tag=sketch_obj.ViewObject.TypeId)
    # sketch_obj.ViewObject.TempoVis = tv
    # if sketch_obj.ViewObject.EditingWorkbench:
    #     tv.activateWorkbench(sketch_obj.ViewObject.EditingWorkbench)
    # if sketch_obj.ViewObject.HideDependent:
    #     tv.hide(tv.get_all_dependent(App.getDocument('Unnamed').getObject('Body'), 'Sketch.'))
    # if sketch_obj.ViewObject.ShowSupport:
    #     tv.show([ref[0] for ref in sketch_obj.Support if not ref[0].isDerivedFrom("PartDesign::Plane")])
    # if sketch_obj.ViewObject.ShowLinks:
    #     tv.show([ref[0] for ref in sketch_obj.ExternalGeometry])
    # tv.hide(sketch_obj)
    # del (tv)
    # # del (sketch_obj)
    #
    # # ActiveSketch = App.getDocument('Unnamed').getObject('Sketch')
    # if sketch_obj.ViewObject.RestoreCamera:
    #     sketch_obj.ViewObject.TempoVis.saveCamera()
    sketch_obj.delGeometries([0, 1])

    return vertexes


def project_on_sketch(ndarray_of_coords: numpy.ndarray,
                      root_pt_of_sketch: numpy.ndarray,
                      unit_H_axis_3D: numpy.ndarray,
                      unit_V_axis_3D: numpy.ndarray
                      ):
    """
    将以ndarray形式的坐标（一行表示一个点）投影到指定的草图
    :param ndarray_of_pts:
    :return:
    """
    v_root_pt_to_coords = ndarray_of_coords - root_pt_of_sketch
    X = numpy.dot(v_root_pt_to_coords, unit_H_axis_3D)
    print(X)
    Y = numpy.dot(v_root_pt_to_coords, unit_V_axis_3D)
    return numpy.array([X, Y])


def new_sketch_supported_by_selected_faces(faces_selection_obj  #: Gui.SelectionObject,
                                           ):
    ### Begin command PartDesign_NewSketch
    multifaces = eval(
        faces_selection_obj.FullName)  # 形如(App.getDocument('Unnamed').getObject('BaseFeature'),['Face39','Face41','Face81','Face144',])
    res = OrderedDict()
    for i in range(len(faces_selection_obj.SubObjects)):
        sketch_obj = App.ActiveDocument.Body.newObject('Sketcher::SketchObject', )
        # face_name =  Gui.Selection.getSelectionEx()[0]

        sketch_obj.Support = (multifaces[0], multifaces[1][i])  # (App.ActiveDocument.BaseFeature, ['Face144', ])
        sketch_obj.MapMode = 'FlatFace'
        App.ActiveDocument.recompute()
        # App.ActiveDocument.setEdit(App.ActiveDocument.Body, 0, '%s.' % (sketch_obj.Name))
        # ActiveSketch = App.getDocument('Unnamed').getObject('Sketch')
        # tv = Show.TempoVis(App.ActiveDocument, tag=ActiveSketch.ViewObject.TypeId)
        # ActiveSketch.ViewObject.TempoVis = tv
        # if ActiveSketch.ViewObject.EditingWorkbench:
        #     tv.activateWorkbench(ActiveSketch.ViewObject.EditingWorkbench)
        # if ActiveSketch.ViewObject.HideDependent:
        #     tv.hide(tv.get_all_dependent(App.getDocument('Unnamed').getObject('Body'), 'Sketch.'))
        # if ActiveSketch.ViewObject.ShowSupport:
        #     tv.show([ref[0] for ref in ActiveSketch.Support if not ref[0].isDerivedFrom("PartDesign::Plane")])
        # if ActiveSketch.ViewObject.ShowLinks:
        #     tv.show([ref[0] for ref in ActiveSketch.ExternalGeometry])
        # tv.hide(ActiveSketch)
        # del (tv)
        # del (ActiveSketch)
        #
        # import PartDesignGui
        # ActiveSketch = App.getDocument('Unnamed').getObject('Sketch')
        # if ActiveSketch.ViewObject.RestoreCamera:
        #     ActiveSketch.ViewObject.TempoVis.saveCamera()

        ### End command PartDesign_NewSketch
        # Gui.Selection.clearSelection()

        # 关闭草图
        # App.ActiveDocument.resetEdit()
        App.ActiveDocument.recompute()
        # tv = sketch_obj.ViewObject.TempoVis
        # if tv:
        #     tv.restore()
        # sketch_obj.ViewObject.TempoVis = None
        # del (tv)

        # Gui.Selection.addSelection('Unnamed','Body','Sketch001.')
        App.ActiveDocument.recompute()
        res[faces_selection_obj.SubElementNames[i]] = sketch_obj
    return res


def draw_sketch(coords_2D: numpy.ndarray, sketch_obj):
    """
    将给定的坐标作为闭合折线画在草图上
    :param coords_2D:
    :return:
    """
    N = len(coords_2D)
    for i in range(N):
        sketch_obj.addGeometry(
            Part.LineSegment(App.Vector(*coords_2D[i % N], 0), App.Vector(*coords_2D[(i + 1) % N], 0)), False)


def farthest_pt_index(coords_3D: numpy.ndarray):
    """

    :param coords_3D: shape (N,3), 每行表示一个三维坐标；首尾点不重合。
    :return: 距离最远的两个相邻点的坐标
    """

    coords_3D_ = numpy.vstack([coords_3D, coords_3D[0]]) # 首尾点重合，用于计算收尾距离
    distances = (numpy.diff(coords_3D_, axis=0) ** 2).sum(axis=1)
    i = numpy.argmax(distances)
    return i, (i + 1) % len(coords_3D)


def random_error_Gaussian(coords: numpy.ndarray, sigma):
    errors = numpy.random.normal(0, sigma, size=coords.shape)
    return coords + errors


def pad_sketch(sketch_obj, ):
    # Gui.Selection.addSelection('Unnamed','Body','Sketcher__SketchObject001.')
    # Gui.Selection.clearSelection()
    # Gui.Selection.addSelection('Unnamed','Body','Sketcher__SketchObject.')
    ### Begin command PartDesign_Pad
    pad = App.ActiveDocument.Body.newObject('PartDesign::Pad',  # 'Pad'
                                            )
    pad.Profile = sketch_obj
    pad.Length = 200.0
    #
    # App.getDocument('Unnamed').getObject('Sketcher__SketchObject').Visibility = False
    # App.ActiveDocument.recompute()
    # App.getDocument('Unnamed').getObject('Pad').ViewObject.ShapeColor = getattr(
    #     App.getDocument('Unnamed').getObject('Pocket').getLinkedObject(True).ViewObject, 'ShapeColor',
    #     App.getDocument('Unnamed').getObject('Pad').ViewObject.ShapeColor)
    # App.getDocument('Unnamed').getObject('Pad').ViewObject.LineColor = getattr(
    #     App.getDocument('Unnamed').getObject('Pocket').getLinkedObject(True).ViewObject, 'LineColor',
    #     App.getDocument('Unnamed').getObject('Pad').ViewObject.LineColor)
    # App.getDocument('Unnamed').getObject('Pad').ViewObject.PointColor = getattr(
    #     App.getDocument('Unnamed').getObject('Pocket').getLinkedObject(True).ViewObject, 'PointColor',
    #     App.getDocument('Unnamed').getObject('Pad').ViewObject.PointColor)
    # App.getDocument('Unnamed').getObject('Pad').ViewObject.Transparency = getattr(
    #     App.getDocument('Unnamed').getObject('Pocket').getLinkedObject(True).ViewObject, 'Transparency',
    #     App.getDocument('Unnamed').getObject('Pad').ViewObject.Transparency)
    # App.getDocument('Unnamed').getObject('Pad').ViewObject.DisplayMode = getattr(
    #     App.getDocument('Unnamed').getObject('Pocket').getLinkedObject(True).ViewObject, 'DisplayMode',
    #     App.getDocument('Unnamed').getObject('Pad').ViewObject.DisplayMode)
    # Gui.getDocument('Unnamed').setEdit(App.getDocument('Unnamed').getObject('Body'), 0, 'Pad.')
    pad.UseCustomVector = 1
    pad.Direction = (0, 0, -1)
    pad.Type = 0
    pad.UpToFace = None
    pad.Reversed = 0
    pad.Midplane = 0
    pad.Offset = 0
    Gui.Selection.clearSelection()
    ### End command PartDesign_Pad
    # Gui.Selection.clearSelection()
    App.ActiveDocument.recompute()


def export_as_step(body):
    """

    :param body: can be App.ActiveDocument.Body by default
    :return:
    """
    __objs__ = []
    __objs__.append(body)
    import ImportGui
    ImportGui.export(__objs__, r"F:/changeworld/FreeCADTools/test/Unnamed-Body.step")
    del __objs__


print("Hello")
print("假设已经挖掉了大部分旧的极面（即，有一个pocket。其应从削斜的端面开始，至少到z=0的位置，但不能完全贯穿，否则FreeCAD将无法在一个Body内建模）")
faces_sel_object = Gui.Selection.getSelectionEx()[0]
dict_name_to_face = {faces_sel_object.SubElementNames[i]: faces_sel_object.SubObjects[i] for i in
                     range(len(faces_sel_object.SubElementNames))}
faces = faces_sel_object.SubObjects  # get_selected_face()
coords_multiface = outwire_vertexes_to_ndarray(faces_to_list_of_OuterWire_OrderedVertexes(faces))
sketches = new_sketch_supported_by_selected_faces(faces_sel_object)
farthest_pt_indexes = [farthest_pt_index(coords_multiface[i]) for i in range(len(faces))]
print(farthest_pt_indexes)
for i in range(len(faces)):
    coords = coords_multiface[i]
    sketch = list(sketches.values())[i]
    pt_on_H_axis, root_pt, pt_on_V_axis = get_coord_system_of_sketch(sketch)
    print(root_pt.X, root_pt.Y, root_pt.Z)
    print(pt_on_H_axis.X, pt_on_H_axis.Y, pt_on_H_axis.Z)
    print(pt_on_V_axis.X, pt_on_V_axis.Y, pt_on_V_axis.Z)
    root_pt_arr = numpy.array([root_pt.X, root_pt.Y, root_pt.Z])
    coords_projected = project_on_sketch(
        coords,
        root_pt_arr,
        numpy.array([pt_on_H_axis.X, pt_on_H_axis.Y, pt_on_H_axis.Z]) - root_pt_arr,
        numpy.array([pt_on_V_axis.X, pt_on_V_axis.Y, pt_on_V_axis.Z]) - root_pt_arr).transpose([1, 0])
    mask = numpy.ones(len(coords_projected), bool)
    for ii in range(len(farthest_pt_indexes[i])):
        mask[farthest_pt_indexes[i][ii]] = False
    print(mask)
    coords_projected[mask] = random_error_Gaussian(coords_projected[mask], 105e-3)  #
    print('add errors')
    draw_sketch(coords_projected, sketch)
    pad_sketch(sketch)
    print(coords_projected)
    print(coords_projected.shape)
    export_as_step(App.ActiveDocument.Body)
print('end!')
